#include "hexapodServoIO.h"
#include "rtthread.h"
#include "drv_st485.h"
#include "bsp.h"

const int16_t int16_t_NONE = 0;
const uint8_t SERVO_FRAME_HEADER = 0x55;
const uint8_t SERVO_MOVE_TIME_WRITE = 1;
const uint8_t SERVO_MOVE_TIME_READ = 2;
const uint8_t SERVO_MOVE_TIME_WAIT_WRITE = 7;
const uint8_t SERVO_MOVE_TIME_WAIT_READ = 8;
const uint8_t SERVO_MOVE_START = 11;
const uint8_t SERVO_MOVE_STOP = 12;
const uint8_t SERVO_ID_WRITE = 13;
const uint8_t SERVO_ID_READ = 14;
const uint8_t SERVO_ANGLE_OFFSET_ADJUST = 17;
const uint8_t SERVO_ANGLE_OFFSET_WRITE = 18;
const uint8_t SERVO_ANGLE_OFFSET_READ = 19;
const uint8_t SERVO_ANGLE_LIMIT_WRITE = 20;
const uint8_t SERVO_ANGLE_LIMIT_READ = 21;
const uint8_t SERVO_VIN_LIMIT_WRITE = 22;
const uint8_t SERVO_VIN_LIMIT_READ = 23;
const uint8_t SERVO_TEMP_MAX_LIMIT_WRITE = 24;
const uint8_t SERVO_TEMP_MAX_LIMIT_READ = 25;
const uint8_t SERVO_TEMP_READ = 26;
const uint8_t SERVO_VIN_READ = 27;
const uint8_t SERVO_POS_READ = 28;
const uint8_t SERVO_OR_MOTOR_MODE_WRITE = 29;
const uint8_t SERVO_OR_MOTOR_MODE_READ = 30;
const uint8_t SERVO_LOAD_OR_UNLOAD_WRITE = 31;
const uint8_t SERVO_LOAD_OR_UNLOAD_READ = 32;
const uint8_t SERVO_LED_CTRL_WRITE = 33;
const uint8_t SERVO_LED_CTRL_READ = 34;
const uint8_t SERVO_LED_ERROR_WRITE = 35;
const uint8_t SERVO_LED_ERROR_READ = 36;

// 计算校验和
uint8_t checkSum(uint8_t buf[], uint8_t len) {
    int16_t sum = 0x0000;
    // 去掉命令开头的两个 0x55
    for (uint8_t i = 0; i < len; i++) {
        sum += *(buf + i);
    }
    // 取反
    sum = ~sum;
	  uint8_t cnt = sum & 0xff;
    return cnt;
}

// 同时向多个舵机写指令
void serialServoWriteMultiCmd(uint8_t cmd, int16_t data[][4], uint8_t num) {
	for (uint8_t i = 0; i < num; i++) {
		uint8_t id = data[i][0] & 0xff;
		uint8_t w_cmd = data[i][1] & 0xff;
		int16_t dat1 = data[i][2];
		int16_t dat2 = data[i][3];
		serialSerroWriteCmd(id, w_cmd, dat1, dat2);	
		// ms
		rt_thread_delay(1);
	}
}

// 写指令
void serialSerroWriteCmd(uint8_t id, uint8_t w_cmd, int16_t dat1, int16_t dat2) {
    uint8_t buf[10] = {int16_t_NONE};
		// 帧头
    buf[0] = 0x55; 
    buf[1] = 0x55; 
		// 设备号
    buf[2] = id;
    // 指令
    buf[4] = w_cmd;

    if (dat1 == int16_t_NONE && dat2 == int16_t_NONE) {
		// 指令长度 去掉帧头
        buf[3] = 3;
		// 校验
		buf[5] = checkSum(&buf[2], buf[3]);
    } else if (dat1 != int16_t_NONE && dat2 == int16_t_NONE) {
		// 指令长度 去掉帧头
        buf[3] = 4;
		// 数据 
		buf[5] = dat1 & 0xff;
		// 校验
		buf[6] = checkSum(&buf[2], buf[3]);	
    } else if (dat1 != int16_t_NONE && dat2 != int16_t_NONE) {
		// 指令长度 去掉帧头
        buf[3] = 7;
		// 数据 
		buf[5] = 0xff & dat1;
		buf[6] = 0xff & (dat1 >> 8);
		buf[7] = 0xff & dat2;
		buf[8] = 0xff & (dat2 >> 8);
		// 校验
		buf[9] = checkSum(&buf[2], buf[3]);
    }
	
    // 写入串口  
		uint8_t length = buf[3] + 3;
			
		rt_mq_send(rt_mq_STmotor_send,buf,length);
		//	STUartxDMASendData(buf,length);

}

// 发送读取命令
void serialServoReadCmd(uint8_t id, uint8_t r_cmd) {
    uint8_t buf[10] = {0};
	  // 帧头
    buf[0] = 0x55; 
    buf[1] = 0x55;
	// 设备号
    buf[2] = id;
	// 指令长度
    buf[3] = 3;    
	// 读指令
    buf[4] = r_cmd;
    buf[5] = checkSum(&buf[2], buf[3]);
  // 写入串口  
	// uint8_t length = buf[3] + 3;

	rt_mq_send(rt_mq_STmotor_send,buf,10);
//	STUartxDMASendData(buf,length);		
	// ms
//	rt_thread_delay(3);
}

// 1. 发送读取命令
// 2. 获取指定读取命令的数据
void serialServoGetMsg(uint8_t cmd, int16_t* value) {
    uint8_t recv_data[ST_UARTREC_MSG_SIZE] = {int16_t_NONE};
    // 清空串口接收缓存
    // 获取串口数据
		// wait and get data from queue for 10 ms, if overtime ,return  -RT_ETIMEOUT,
		if( rt_mq_recv(rt_mq_STmotor_rec,recv_data,ST_UARTREC_MSG_SIZE,10) != -RT_ETIMEOUT) {
				if (recv_data[0] == 0x55 && recv_data[1] == 0x55 && recv_data[4] == cmd) {
						uint8_t dat_len = recv_data[3];
						if (dat_len == 4) {
								*(value + 0) = recv_data[5] & 0xffff;
						} else if (dat_len == 5) {
								*(value + 0) = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)));
						} else if (dat_len == 7) {
								*(value + 0) = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)));
								*(value + 1) = 0xffff & (recv_data[7] | (0xff00 & (recv_data[8] << 8)));
						}
				}
		}
}
